#include <ros/ros.h> 
#include <std_msgs/String.h> 
#include <std_msgs/Empty.h> 
#include <geometry_msgs/Twist.h>
#include <string>
   
ros::Publisher read_pub; 
static ros::Publisher motion_pub;
int a=0;
std_msgs::String result;
using namespace std;
void motionCallback(const std_msgs::String::ConstPtr& msg)
{
	ROS_INFO_STREAM("I heard: "<< msg->data);
        geometry_msgs::Twist motion;
        if(msg->data=="f")                 //0
      {
            motion.linear.x =0.2;
            motion.linear.y =0.0;
            motion.linear.z =0.0;
            
            motion.angular.z=0;
            motion.angular.z=0;
            motion.angular.z=0;
            motion_pub.publish(motion);
            a++;
      } 
      else if(msg->data=="r")                //1
      {
            motion.linear.x =0.0;
            motion.linear.y =0.0;
            motion.linear.z =0.0;
            
            motion.angular.z=0;
            motion.angular.z=0;
            motion.angular.z=-1.5;
            motion_pub.publish(motion);
            a++;
      }
      else if(msg->data=="l"){                    //2
            motion.linear.x =0.0;
            motion.linear.y =0.0;
            motion.linear.z =0.0;
            
            motion.angular.z=0;
            motion.angular.z=0;
            motion.angular.z=1.5;
            motion_pub.publish(motion);
            a++;
      }
      else if(msg->data=="b"){             //3
            motion.linear.x =-0.2;
            motion.linear.y =0.0;
            motion.linear.z =0.0;
            
            motion.angular.z=0;
            motion.angular.z=0;
            motion.angular.z=0.0;
            motion_pub.publish(motion);
            a++; 
      }
      
      result.data = to_string(a);
      read_pub.publish(result);
}

int main(int argc, char **argv)
{

	ros::init(argc, argv, "read_serial");
	ros::NodeHandle n;
	read_pub = n.advertise<std_msgs::String>("/write", 1);
	ros::Subscriber sub = n.subscribe("/read", 1000, motionCallback);
        motion_pub  =n.advertise<geometry_msgs::Twist>("/mobile_base/commands/velocity", 100);

	ros::spin();
        return 0;
}
